{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 6: Localization and Mapping\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5f0c609c",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import google.colab\n",
    "    print('Running on CoLab')\n",
    "    COLAB = True\n",
    "    #!pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install git+https://github.com/petercorke/robotics-toolbox-python@future\n",
    "    !pip install spatialmath-python>=1.1.5\n",
    "    !pip install --no-deps rvc3python\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "b21f6c9e",
   "metadata": {},
   "source": [
    "There are some minor code changes compared to the book. These are to support \n",
    "animations in the Jupyter environment. Animations can be made using `FuncAnimation` and\n",
    "the `widget` backend, but with Colab these can be very unsmooth.  Instead we render the \n",
    "animations to HTML5 and render them locally in the browser."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7173cbff",
   "metadata": {},
   "source": [
    "# 6.1 Dead Reckoning using Odometry\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "44474c30",
   "metadata": {},
   "source": [
    "## 6.1.1 Modeling the Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "359c2eb9",
   "metadata": {},
   "outputs": [],
   "source": [
    "V = np.diag([0.02, np.deg2rad(0.5)]) ** 2;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c8ba37e",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Bicycle(covar=V, animation=\"car\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f7ce323",
   "metadata": {},
   "outputs": [],
   "source": [
    "odo = robot.step((1, 0.3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f52ca878",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e4bd4eef",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.f([0, 0, 0], odo)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0884d08f",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.control = RandomPath(workspace=10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ef353dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "# robot.run(T=10);\n",
    "\n",
    "html = robot.run_animation(T=20, format=\"html\");\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3bb83c3d",
   "metadata": {},
   "source": [
    "## 6.1.2 Estimating Pose\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0a699eca",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.Fx([0, 0, 0], [0.5, 0.1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27545a4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "x_sdev = [0.05, 0.05, np.deg2rad(0.5)];\n",
    "P0 = np.diag(x_sdev) ** 2;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d17277fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf = EKF(robot=(robot, V), P0=P0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2a852ad2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ekf.run(T=20);\n",
    "\n",
    "html = ekf.run_animation(T=20, format=\"html\");\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5f9ca8ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.plot_xy(color=\"b\")\n",
    "ekf.plot_xy(color=\"r\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dbf95a82",
   "metadata": {},
   "outputs": [],
   "source": [
    "P150 = ekf.get_P(150)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad76507e",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.sqrt(P150[0, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1045f6ed",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf.plot_xy(color=\"r\")\n",
    "ekf.plot_ellipse(filled=True, facecolor=\"g\", alpha=0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16a96290",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = ekf.get_t();\n",
    "pn = ekf.get_Pnorm();\n",
    "plt.plot(t, pn);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "747aafb0",
   "metadata": {},
   "source": [
    "# 6.2 Localizing with a Landmark Map\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ce7bfd9",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = LandmarkMap(20, workspace=10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8095dabf",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "88405804",
   "metadata": {},
   "outputs": [],
   "source": [
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8686311",
   "metadata": {},
   "outputs": [],
   "source": [
    "sensor = RangeBearingSensor(robot=robot, map=map, covar=W,  \n",
    "           angle=[-pi/2, pi/2], range=4, animate=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8aa97d9e",
   "metadata": {},
   "outputs": [],
   "source": [
    "z, i = sensor.reading()\n",
    "# z\n",
    "# i\n",
    "print(f\"landmark {i} at {z}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dd540b0d",
   "metadata": {},
   "outputs": [],
   "source": [
    "map[15]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "93c1639c",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = LandmarkMap(20, workspace=10);\n",
    "V = np.diag([0.02, np.deg2rad(0.5)]) ** 2\n",
    "robot = Bicycle(covar=V, animation=\"car\");\n",
    "robot.control = RandomPath(workspace=map, seed=0)\n",
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2\n",
    "sensor = RangeBearingSensor(robot=robot, map=map, covar=W, \n",
    "           angle=[-pi/2, pi/2], range=4, seed=0, animate=True);\n",
    "P0 = np.diag([0.05, 0.05, np.deg2rad(0.5)]) ** 2;\n",
    "ekf = EKF(robot=(robot, V), P0=P0, map=map, sensor=(sensor, W));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0ed6fddf",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ekf.run(T=20)\n",
    "html = ekf.run_animation(T=20, format=\"html\")\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cece94da",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot()\n",
    "robot.plot_xy();\n",
    "ekf.plot_xy();\n",
    "ekf.plot_ellipse()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9675fc6a",
   "metadata": {},
   "source": [
    "# 6.3 Creating a Landmark Map\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1812cd5f",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = LandmarkMap(20, workspace=10, seed=0);\n",
    "robot = Bicycle(covar=V, animation=\"car\");\n",
    "robot.control = RandomPath(workspace=map);\n",
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2\n",
    "sensor = RangeBearingSensor(robot=robot, map=map, covar=W, \n",
    "           range=4, angle=[-pi/2, pi/2], animate=True);\n",
    "ekf = EKF(robot=(robot, None), sensor=(sensor, W));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c2a9c04d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ekf.run(T=100);\n",
    "\n",
    "html = ekf.run_animation(T=100, format=\"html\")\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "283f1d2b",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot();\n",
    "ekf.plot_map();\n",
    "robot.plot_xy();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ffc25a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf.landmark(10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db159f24",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf.x_est[24:26]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "190b3ffb",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf.P_est[24:26, 24:26]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e849e4cb",
   "metadata": {},
   "source": [
    "# 6.4 Simultaneous Localization and Mapping\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "646f1456",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = LandmarkMap(20, workspace=10);\n",
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2 \n",
    "robot = Bicycle(covar=V, x0=(3, 6, np.deg2rad(-45)), \n",
    "          animation=\"car\");\n",
    "robot.control = RandomPath(workspace=map);\n",
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2\n",
    "sensor = RangeBearingSensor(robot=robot, map=map, covar=W, \n",
    "           range=4, angle=[-pi/2, pi/2], animate=True);\n",
    "P0 = np.diag([0.05, 0.05, np.deg2rad(0.5)]) ** 2;\n",
    "ekf = EKF(robot=(robot, V), P0=P0, sensor=(sensor, W));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3375b181",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ekf.run(T=40);\n",
    "\n",
    "html = ekf.run_animation(T=40, format=\"html\")\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21c85c60",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot();       # plot true map\n",
    "robot.plot_xy();  # plot true path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99da68b5",
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf.plot_map();      # plot estimated landmark position\n",
    "ekf.plot_ellipse();  # plot estimated covariance\n",
    "ekf.plot_xy();       # plot estimated robot path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25334b1f",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = ekf.get_transform(map)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dae247ac",
   "metadata": {},
   "source": [
    "# 6.5 Pose-Graph SLAM\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "179f091e",
   "metadata": {},
   "source": [
    "NOTE. Minor changes to ensure Jupyter pretty printing of SymPy equations.  Introduced underscore to indicate subscripting, and changed `t` to `theta`\n",
    "which is printed as $\\theta$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "650ecc07",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "xi, yi, ti, xj, yj, tj = sympy.symbols(\"x_i y_i theta_i x_j y_j theta_j\")\n",
    "xm, ym, tm = sympy.symbols(\"x_m y_m theta_m\")\n",
    "xi_e = SE2(xm, ym, tm).inv() * SE2(xi, yi, ti).inv() \\\n",
    "     * SE2(xj, yj, tj);\n",
    "fk = sympy.Matrix(sympy.simplify(xi_e.xyt()));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9b46261f",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ai = sympy.simplify(fk.jacobian([xi, yi, ti]))\n",
    "Ai.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0eb64b44",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ai"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e41e1468",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg = PoseGraph(\"data/pg1.g2o\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8809c441",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg.plot();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0559f1b8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# pg.optimize(animate=True)\n",
    "\n",
    "pg.optimize(animate=False)\n",
    "pg.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30004352",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg = PoseGraph(\"data/killian-small.toro\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c89067b9",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg.plot(text=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57368cc7",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg.optimize()\n",
    "pg.plot(text=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "105d865e",
   "metadata": {},
   "source": [
    "# 6.6 Sequential Monte-Carlo Localization\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0afdcc7f",
   "metadata": {},
   "outputs": [],
   "source": [
    "map = LandmarkMap(20, workspace=10);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1e8b0f41",
   "metadata": {},
   "outputs": [],
   "source": [
    "V = np.diag([0.02, np.deg2rad(0.5)]) ** 2;\n",
    "robot = Bicycle(covar=V, animation=\"car\", workspace=map);\n",
    "robot.control = RandomPath(workspace=map)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aba852a2",
   "metadata": {},
   "outputs": [],
   "source": [
    "W = np.diag([0.1, np.deg2rad(1)]) ** 2;\n",
    "sensor = RangeBearingSensor(robot, map, covar=W, plot=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5956ca22",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0fda5cfb",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = np.diag([0.1, 0.1]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "22286164",
   "metadata": {},
   "outputs": [],
   "source": [
    "pf = ParticleFilter(robot, sensor=sensor, R=R, L=L, nparticles=1000, animate=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4fa1e428",
   "metadata": {},
   "outputs": [],
   "source": [
    "# pf.run(T=10);\n",
    "\n",
    "html = pf.run_animation(T=10, format=\"html\");\n",
    "HTML(html)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c1d348d",
   "metadata": {},
   "outputs": [],
   "source": [
    "map.plot();\n",
    "robot.plot_xy();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "37de2b39",
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.plot_xy();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81280ad6",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(pf.get_std()[:100,:]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "965987bf",
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.plot_pdf()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "707fadbe",
   "metadata": {},
   "source": [
    "# 6.8 Application: Lidar\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "70c48b1e",
   "metadata": {},
   "source": [
    "## 6.8.1 Lidar-based Odometry\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2c61c67",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg = PoseGraph(\"data/killian.g2o.zip\", lidar=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1864a99",
   "metadata": {},
   "outputs": [],
   "source": [
    "[r, theta] = pg.scan(100);\n",
    "r.shape\n",
    "theta.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45a7e848",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()\n",
    "plt.polar(theta, r);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ac2979df",
   "metadata": {},
   "outputs": [],
   "source": [
    "p100 = pg.scanxy(100);\n",
    "p101 = pg.scanxy(100);\n",
    "p100.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d6e442b2",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = pg.scanmatch(100, 101);\n",
    "T.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b973f440",
   "metadata": {},
   "outputs": [],
   "source": [
    "pg.time(101) - pg.time(100)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d38c9370",
   "metadata": {},
   "source": [
    "## 6.8.2 Lidar-based Map Building\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7400cc0b",
   "metadata": {},
   "outputs": [],
   "source": [
    "og = OccupancyGrid(workspace=[-100, 250, -100, 250], cellsize=0.1, value=np.int32(0));\n",
    "pg.scanmap(og, maxrange=40)\n",
    "og.plot(cmap=\"gray\")"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "RVC3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
